﻿// 粗差处理相关函数，数据探测、抗差估计及改进

using MathNet.Numerics.LinearAlgebra;
using System;
using System.Collections.Generic;
using static AdjPlane.AdjCmn;

namespace AdjPlane
{
    using mat = Matrix<double>;
    using obs_dic = Dictionary<string, double>;

    // 将DSP视为只有保留和剔除区域的权函数
    public enum WeightFunc { DataSnooping, Huber, IGG1, IGG3 }   //抗差估计权函数
    public enum Robust0 { LS, L1, LMS, LTS,CLTS }           //残差稳健初值计算
    public struct DspN
    {
        public string beg;
        public string end;
        public string type;
    }
    // 粗差探测时剩余观测值的序号
    public struct DSP
    {
        public int[] idxL;
        public int[] idxS;
        public List<DspN> names;
    }
    public struct Robust_t
    {
        public WeightFunc WgtFunc; // 抗差估计权函数
        public double K0,K1; // 表示抗差估计权函数的临界值，Huber只需K0，IGG需要K0,K1
        public Robust0 robust0; // 观测值残差初值的计算方法(平差一次时的残差，用于抗差估计判断)
    }

    //抗差估计
    public partial class Plane
    {
        const double MIN = 1e-2;//粗差降权因子
        readonly int dd = 5;
        int n0, n1, n2;
        DSP dsp; //剔除后的两类观测值的序号

        //从N个数中一次性抽取M个数，返回M个数的序号
        public static int[] Sample(int N, int M)
        {
            List<int> nums = new List<int>();
            for (int i = 0; i < N; i++)
            {
                nums.Add(i);  // 创建包含N个数的索引列表
            }
            Random random = new Random();
            List<int> sample = new List<int>();
            while (sample.Count < M)
            {
                int index = random.Next(nums.Count);  // 生成随机索引
                if (!sample.Contains(index))
                {
                    sample.Add(nums[index]);
                }
            }
            sample.Sort();
            int[] ret = sample.ToArray();
            return ret;
        }
        //从数组C中抽样M个数，返回这些数在原数组中的序号
        public static int[] Sample<T>(T[] C, int M)
        {
            Random random = new Random();
            List<T> sample = new List<T>();
            List<int> sample_index = new List<int>();
            while (sample.Count < M)
            {
                int index = random.Next(C.Length);  // 生成随机索引
                if (!sample.Contains(C[index]))
                {
                    sample.Add(C[index]);
                    sample_index.Add(index);
                }
            }
            sample_index.Sort();
            int[] ret = sample_index.ToArray();
            return ret;
        }
        //随机重复抽样
        //  N       I:总样本数
        //  M       I:抽样数
        //  count   I:有放回的重复抽样次数
        //  return:每次抽样的样本在总样本中的序列号
        public static List<int[]> MultiSample(int N, int M, int count)
        {
            int[] one = new int[M];
            List<int[]> sum = new List<int[]>();
            Random rd = new Random();
            for (int i = 0; i < count; i++)
            {
                one = Sample(N, M);
                sum.Add(one);
            }
            sum.Sort((x, y) => x[0].CompareTo(y[0]));
            return sum;
        }
        //单位权中误差
        public static double GetSigma(mat V, mat Qvv)
        {
            int num = V.RowCount;
            mat K = mat.Build.Dense(num, 1, 0);
            double[] diag = Qvv.Diagonal().ToArray();
            for (int i = 0; i < num; i++)
                K[i, 0] = Math.Abs(V[i, 0]) / Math.Sqrt(Math.Abs(Qvv[i, i]));
            double[] arr = AdjCmn.ColArray(K.ToArray());
            double sigma0 = 1.4826 * AdjCmn.Median(arr);
            return sigma0;
        }
        //获取标准化残差
        public static mat GetVi(mat V, double sigma0, mat Qvv)
        {
            int num = V.RowCount;
            mat Vi = Newmat(num, 1);
            double[] arr = Qvv.Diagonal().ToArray();
            for (int i = 0; i < num; i++)
            {
                double qvvi = Math.Abs(arr[i]);
                if (qvvi < 1e-10) 
                    qvvi += 1e-10;  // 处理实际数据时出现极端情况
                Vi[i, 0] = V[i, 0] / (sigma0 * Math.Sqrt(qvvi));
            }
            return Vi;
        }
        //MAD法获取标准化残差
        public static mat GetVi(mat V)
        {
            int num = V.RowCount;
            mat Vi = Newmat(num, 1);
            double[] arr = ColArray(V.ToArray());
            double med = Median(arr);
            double mad = 1.4826 * Mad(arr);
            for (int i = 0; i < num; i++)
                Vi[i, 0] = (V[i, 0] - med) / mad;
            return Vi;
        }
        //根据Cstep做LTS估计，获得抗差估计的初值
        mat LTS_Cstep(mat B, mat L, mat P)
        {
            int m = (int)((n + t) * 0.5);
            List<int[]> result = new List<int[]>();
            result = MultiSample(n, m, 1000);
            mat X = mat.Build.Dense(t, 1, 0);
            mat Xm = mat.Build.Dense(t, 1, 0);
            double vmin = 99999;
            double v_lts = 0;
            //List<double> Cstep = new List<double>();
            for (int i = 0; i < result.Count; i++)
            {
                int[] indexs = result[i];
                for (int k = 0; k < 2; k++)
                {
                    var Bm = SplitMat2(B, indexs);
                    if (Bm.Rank() < Bm.ColumnCount)
                        continue;
                    var Lm = SplitMat2(L, indexs);
                    var Pm = SplitMat1(P, indexs);
                    var Nm = Bm.Transpose() * Pm * Bm;
                    var Wm = Bm.Transpose() * Pm * Lm;
                    Xm = Nm.Inverse() * Wm;
                    var Vn = B * Xm - L;
                    double[] arr = ColArray(Vn.ToArray());
                    //indexs = Cstep(arr, m, out v_lts);
                }
                if (v_lts < vmin)
                {
                    vmin = v_lts;
                    X = Xm;
                }
            }
            return X;
        }
        //组合(适用于小样本，大量数据可改用随即重复抽样）
        public static void Combine(int n, int h, int[] a, int[] b, ref List<int[]> result)
        {
            for (int i = h; i <= n; i++)
            {
                b[h - 1] = i - 1;
                if (h > 1)
                    Combine(i - 1, h - 1, a, b, ref result);
                else
                {
                    int[] c = new int[b.Length];
                    for (int j = 0; j < b.Length; j++)
                        c[j] = a[b[j]];
                    result.Add(c);
                }
            }
        }
        //计算权函数
        double GetWi(Robust_t rbopt, double vi)
        {
            double k0 = rbopt.K0;
            double k1 = rbopt.K1;

            vi = Math.Abs(vi);
            if (vi < 1e-10) vi += 1e-10;

            switch (rbopt.WgtFunc)
            {
                case WeightFunc.DataSnooping:
                    if (vi <= k0) return 1;
                    return MIN;
                case WeightFunc.Huber:
                    if (vi <= k0) return 1;
                    return k0 / vi;
                case WeightFunc.IGG1:
                    if (vi <= k0) return 1;
                    if (vi > k1) return MIN;
                    return k0 / vi;
                case WeightFunc.IGG3:
                    if (vi <= k0) return 1;
                    if (vi > k1) return MIN;
                    double a = (k1 - vi) / (k1 - k0);
                    return k0 / vi * a * a;
                default: return 1;
            }
        }
        //抗差方差分量估计
        void RobustHelmert(Proc_t popt,mat V,double sigma,mat Qvv,mat P_,ref mat PP,ref mat Wi)
        {
            //1.Robust
            int ch = 2;
            if (ch == 1) //----1分别抗差
            {
                var Qvv1 = Qvv.SubMatrix(0, nl, 0, nl);
                var Qvv2 = Qvv.SubMatrix(nl, ns, nl, ns);
                var P1_ = P_.SubMatrix(0, nl, 0, nl);
                var P2_ = P_.SubMatrix(nl, ns, nl, ns);
                var PP1 = PP.SubMatrix(0, nl, 0, nl);
                var PP2 = PP.SubMatrix(nl, ns, nl, ns);
                var WiL = Wi.SubMatrix(0, nl, 0, 1);
                var WiS = Wi.SubMatrix(nl, ns, 0, 1);
                RobustEst(popt.RobustOpt, V1, sigL, Qvv1, P1_, ref PP1, ref WiL);
                RobustEst(popt.RobustOpt, V2, sigS, Qvv2, P2_, ref PP2, ref WiS);
                PP = PP1.DiagonalStack(PP2);
                Wi = WiL.Stack(WiS);
            }
            else   //----2整体抗差
            {
                RobustEst(popt.RobustOpt, V, sigma, Qvv, P_, ref PP, ref Wi);
            }
            //2.HVCE
            var P1 = PP.SubMatrix(0, nl, 0, nl);
            var P2 = PP.SubMatrix(nl, ns, nl, ns);
            int hh = 0;
            double ql, qs;
            var Ninv = N.Inverse();
            var s1 = Ninv * N1;
            var s2 = Ninv * N2;
            double r1 = nl - s1.Trace();
            double r2 = ns - s2.Trace();
            while (true)
            {
                ql = (V1.Transpose() * P1 * V1)[0, 0] / r1;
                qs = (V2.Transpose() * P2 * V2)[0, 0] / r2;
                P1 = ql / ql * P1;  //权阵迭代
                P2 = ql / qs * P2;
                double eps = Math.Abs(ql / qs - 1);
                if (eps < popt.Eps_VCE || hh == popt.IterMax_VCE) //结束方差分量估计迭代
                    break;
                VCE_t cov = new VCE_t
                {
                    count = iter_VCE,
                    ql = ql,
                    qs = qs
                };
                iter_VCE++;
                cov_res.Add(cov);//存储方差分量估计的定权结果
                hh++;
            }
            PP = P1.DiagonalStack(P2);
        }
        /// <summary>
        /// 抗差估计定权
        /// </summary>
        /// <param name="rbopt">抗差估计参数</param>
        /// <param name="V">观测值改正数</param>
        /// <param name="sigma">单位权中误差</param>
        /// <param name="Qvv">观测值协因数阵</param>
        /// <param name="P">权阵</param>
        /// <param name="PP">等价权</param>
        /// <param name="Wi">等价权函数</param>
        void RobustEst(Robust_t rbopt,mat V, double sigma, mat Qvv,
            mat P,ref mat PP,ref mat Wi)
        {
            // 求解标准化残差
            var Vi = GetVi(V, sigma, Qvv);
            for (int i = 0; i < V.RowCount; i++)
            {
                double vi = Vi[i, 0]; 
                Wi[i, 0] *= GetWi(rbopt, vi); // 权函数选择
                PP[i, i] = P[i, i] * Wi[i, 0];
            }
        }

        // 最小一乘估计，没实现
        // ref [10]
        mat GetRobustX_By_L1(mat B, mat L, mat P)
        {
            var ret = Newmat(t, 1);
            return ret;
        }
        //从A中抽取第a[i]行，重新组建矩阵
        public static mat SplitMat2(mat A, int[] a)
        {
            int i, j, k, m = a.Length, n = A.ColumnCount;
            mat B = Newmat(m, n);
            for (i = 0; i < m; i++)
            {
                k = a[i];
                for (j = 0; j < n; j++)
                {
                    B[i, j] = A[k, j];
                }
            }
            return B;
        }
        // 在P阵中抽取a[i]作为新的对角线元素
        public static mat SplitMat1(mat P, int[] a)
        {
            double[] n = new double[a.Length];
            for (int i = 0; i < a.Length; i++)
            {
                n[i] = P[a[i], a[i]];
            }
            return mat.Build.DiagonalOfDiagonalArray(n);
        }
        // 自创的方法，效果太差了！！
        mat GetRobustX_By_CLTS(mat B, mat L, mat P)
        {
            int i,j,k,num;
            double vvmin = 999999, vvmax = 0;
            var N = B.Transpose() * P * B;
            var X = N.Inverse() * B.Transpose() * P * L;
            var V = B * X - L;
            List<int> idN = new List<int>();
            List<int> idn = new List<int>();
            for(i = 0; i < V.RowCount; i++)
            {
                if (V[i, 0] < 1.5 * M0) idN.Add(i);
                else idn.Add(i);
            }
            int n = idn.Count;
            int h = (int)(n / 2);
            int[] b = new int[h];
            List<int[]> res = new List<int[]>();
            Combine(n, h, idn.ToArray(), b, ref res);
            var BnN = SplitMat2(B, idN.ToArray());
            var LnN = SplitMat2(L, idN.ToArray());
            var PnN = SplitMat1(P, idN.ToArray());
            mat Bn,Ln,Pn;
            for (i=k = 0; i < res.Count; i++)
            {
                Bn = BnN; Ln = LnN;  Pn = PnN;
                if (true ||Bn.Rank() < Bn.ColumnCount)
                {
                    var Bnn = SplitMat2(B, res[i]);
                    Bn = BnN.Stack(Bnn);
                    if (Bn.Rank() < Bn.ColumnCount)
                        continue;
                    k++;
                    var Lnn = SplitMat2(L, res[i]);
                    Ln = LnN.Stack(Lnn);
                    var Pnn = SplitMat1(P, res[i]);
                    Pn = PnN.DiagonalStack(Pnn);
                    num = idN.Count + h;
                }
                else num = idN.Count;
                var Nn = Bn.Transpose() * Pn * Bn;
                var Wn = Bn.Transpose() * Pn * Ln;
                var Xn = Nn.Inverse() * Wn;
                V = B * Xn - L;
                for (j = 0; j < V.RowCount; j++)
                {
                    V[j, 0] = V[j, 0] * Math.Sqrt(P[j, j]);
                }
                double[] arr = ColArray(V.ToArray());
                double[] vv = Square(arr);
                Array.Sort(vv);
                var vvh = Dot(vv, vv, num);
                if (vvh < vvmin)
                {
                    vvmin = vvh;
                    X = Xn;
                }
                if (vvh > vvmax)
                {
                    vvmax = vvh;
                }
                if (BnN.Rank() == Bn.ColumnCount) break;
            }
            Print("**有效抽样总数：" + k.ToString());
            Print("**vvmin:" + vvmin.ToString());
            Print("**vvmax:" + vvmax.ToString());
            return X;

        }
        //获得LTS抗差初值
        // ref [9],[11]
        mat GetRobustX_By_LTS(mat B, mat L, mat P)
        {
            int i,j, k;
            List<int[]> res;
            mat X = Newmat(t, 1);
            double vvmin = 99999,vvmax=0;
            
            int h = (int)((n + t) * 0.5); //t是必要观测数
            h = n - 20;
            res = MultiSample(n, h, 1000);
            for (i=k= 0; i < res.Count; i++)
            {
                var Bn = SplitMat2(B, res[i]);
                if (Bn.Rank() < Bn.ColumnCount)
                    continue;
                k++;
                var Ln = SplitMat2(L, res[i]);
                var Pn = SplitMat1(P, res[i]);
                var Nn = Bn.Transpose() * Pn * Bn;
                var Wn = Bn.Transpose() * Pn * Ln;
                var Xn = Nn.Inverse() * Wn;
                var V = B * Xn - L;
                for (j = 0; j < n; j++)
                {
                    V[j, 0] = V[j, 0] * Math.Sqrt(P[j, j]);
                }
                double[] arr = ColArray(V.ToArray());
                double[] vv = AdjCmn.Square(arr);
                Array.Sort(vv);
                var vvh = AdjCmn.Dot(vv, vv, h);
                if (vvh < vvmin)
                {
                    vvmin = vvh;
                    X = Xn;
                }
                if (vvh >vvmax)
                {
                    vvmax = vvh;
                }
            }
            Print("**有效抽样总数：" + k.ToString());
            Print("**vvmin:" + vvmin.ToString());
            Print("**vvmax:" + vvmax.ToString());
            return X;
        }
        //LMS初值
        mat GetRobustX_By_LMS(mat B, mat L, mat P)
        {
            int i,j, k, h;
            h = (int)((n + t) * 0.5);
            h = n - 10;
            List<int[]> result;
            result = MultiSample(n, h, 1000);
            mat X = mat.Build.Dense(t, 1, 0);
            double vvmin = 99999,vvmax=0;
            for (i=k = 0; i < result.Count; i++)
            {
                var Bn = SplitMat2(B, result[i]);
                if (Bn.Rank() < Bn.ColumnCount)
                    continue;
                k++;
                var Ln = SplitMat2(L, result[i]);
                var Pn = SplitMat1(P, result[i]);
                var Nn = Bn.Transpose() * Pn * Bn;
                var Wn = Bn.Transpose() * Pn * Ln;
                var Xn = Nn.Inverse() * Wn;
                var V = B * Xn - L;
                for (j = 0; j < n; j++)
                {
                    V[j, 0] = V[j, 0] * Math.Sqrt(P[j, j]);
                }
                double[] arr = ColArray(V.ToArray());
                double[] vv = AdjCmn.Square(arr);
                Array.Sort(vv);
                var vvm = Median(vv);
                if (vvm < vvmin)
                {
                    vvmin = vvm;
                    X = Xn;
                }
                if (vvm > vvmax)
                {
                    vvmax = vvm;
                }
            }
            Print("**有效抽样总数：" + k.ToString() );
            Print("**vvmin:" + vvmin.ToString());
            Print("**vvmax:" + vvmax.ToString());
            return X;
        }
        //平差
        void RobustAdjust(Proc_t popt)
        {
            int i,iter,nn;//nn剔除粗差后的观测值个数
            mat Wi,Q, Qvv,Qvv1, Qvv2; ;//权函数
            Result = new Dictionary<string, Point2D>(Approx);//近似坐标作为迭代的初值

            A1 = Newmat(nl, t); L1 = Newmat(nl, 1); P1 = Newmat(nl, nl);
            A2 = Newmat(ns, t); L2 = Newmat(ns, 1); P2 = Newmat(ns, ns);
            N1 = Newmat(t, t); W1 = Newmat(t, 1);
            N2 = Newmat(t, t); W2 = Newmat(t, 1);
            N = Newmat(t, t); W = Newmat(t, 1);
            X = Newmat(t, 1); V1 = Newmat(nl, 1); V2 = Newmat(ns, 1);
            P = Newmat(n, n); QX = Newmat(t, t); mat ROC  = Newmat(n, n);

            double[] arr = new double[n];

            M0 = alpha;  qv = M0 * M0; qvl = qv; qvs = qv; //方差因子

            //建立误差方程和法方程并计算
            P1 = GetP1(nl, M0); P2 = GetP2(ns, M0, popt.DisWgt);

            Orientation_Angle(out obs_dic DXJ);//计算定向角
            GetFX1(DXJ, ref A1, ref L1);
            GetJL1(ref A2, ref L2);
            var A = A1.Stack(A2); var L = L1.Stack(L2);
            P = P1.DiagonalStack(P2);
            N = A.Transpose() * P * A;
            switch (popt.RobustOpt.robust0)
            {
                case Robust0.LMS: X = GetRobustX_By_LMS(A, L, P); break;
                case Robust0.LTS: X = GetRobustX_By_LTS(A, L, P); break;
                case Robust0.L1: X = GetRobustX_By_L1(A, L, P); break;
                case Robust0.CLTS:X = GetRobustX_By_CLTS(A, L, P);break;
                default: X = N.Inverse() * A.Transpose() * P * L; break;  // LS
            }
            V1 = A1 * X - L1; V2 = A2 * X - L2; V = V1.Stack(V2);

            Q = P.Inverse();
            Qvv = Q - A * N.Inverse() * A.Transpose();
            Qvv1 = Qvv.SubMatrix(0, nl, 0, nl);
            sigma = GetSigma(V1, Qvv1);
            List<int> idxL = new List<int>();
            for(i = 0; i < nl; i++)
            {
                if (Math.Abs( V1[i,0]) > dd * sigma ) continue;
                idxL.Add(i);
            }
            mat An1 = SplitMat2(A1, idxL.ToArray());
            mat Ln1 = SplitMat2(L1, idxL.ToArray());
            mat Pn1 = SplitMat1(P1, idxL.ToArray());
            mat Nn1 = An1.Transpose() * Pn1 * An1;

            Qvv2 = Qvv.SubMatrix(nl, ns, nl, ns);
            sigma = GetSigma(V2, Qvv2);
            //sigma=Mad(ColArray(V2.ToArray())) * 1.4826;
            List<int> idxS = new List<int>();
            for (i = 0; i < ns; i++)
            {
                if (V2[i, 0] > dd * sigma) continue;
                idxS.Add(i);
            }
            mat An2 = SplitMat2(A2, idxS.ToArray());
            mat Ln2 = SplitMat2(L2, idxS.ToArray());
            mat Pn2 = SplitMat1(P2, idxS.ToArray());
            mat Nn2 = An2.Transpose() * Pn2 * An2;

            mat Nn = Nn1 + Nn2;

            dsp = new DSP();
            dsp.names = new List<DspN>();
            if (Nn.Rank() == t && popt.WgtDef==Weight.DataSnooping)
            {
                //Partial LS粗差探测 
                dsp.idxL = idxL.ToArray();
                dsp.idxS = idxS.ToArray();

                A1 = Newmat(nl, t); L1 = Newmat(nl, 1); P1 = Newmat(nl, nl);
                A2 = Newmat(ns, t); L2 = Newmat(ns, 1); P2 = Newmat(ns, ns);
                N = Newmat(t, t); N1 = Newmat(t, t); 
                A1 = An1; L1 = Ln1; P1 = Pn1;
                A2 = An2; L2 = Ln2; P2 = Pn2;
                N1 = Nn1;N2 = Nn2;N = N1 + N2;
                V1 = A1 * X - L1;V2 = A2 * X - L2;V = V1.Stack(V2);
                A = A1.Stack(A2);
                P = P1.DiagonalStack(P2);
                Q = P.Inverse();
                Qvv = Q - A * N.Inverse() * A.Transpose();
                nn = V.RowCount;
            }
            else
            {
                //抗差估计所需的初始元素
                global_popt = popt;
                P = P1.DiagonalStack(P2);
                Q = P.Inverse();//协因数的初值
                Q1 = P1.Inverse();
                Q2 = P2.Inverse();
                A = A1.Stack(A2);

                //求Qvv和sigma:抗差估计需要
                Qvv = Q - A * N.Inverse() * A.Transpose();
                arr = ColArray(V.ToArray());
                Qvv1 = Qvv.SubMatrix(0, nl, 0, nl);
                Qvv2 = Qvv.SubMatrix(nl, ns, nl, ns);
                sigma = GetSigma(V, Qvv);
                sigL = GetSigma(V1, Qvv1);
                sigS = AdjCmn.Mad(ColArray(V2.ToArray())) * 1.4826;
                nn = n;
            }
            var Xpre = X;
            //外循环坐标迭代
            int ch = 1;
            // P表示观测值的权，是一个常量，抗差估计时改变PP等价权
            mat PP = Eye(n);//等价权初值
            Wi = mat.Build.Dense(n, 1, 1);//权因子初值赋为1
            // 在测试过程中发现，sigma如果每次迭代都更新的话，会导致过度剔除粗差
            switch (ch)
            {
                case 1: sigma = GetSigma(V, Qvv); break;
                case 2: sigma = 1.4826 * Mad(arr); break;
                case 3: sigma = 1.4826 * (1 + 5 / r) * Math.Sqrt(Median(Square(arr))); break;
                default: sigma = Math.Sqrt((V.Transpose() * PP * V)[0, 0] / (nn - t)); break;
            }
            for (iter=0;iter<popt.IterMax_X;iter++)
            {
                //Print(sigma);
                switch (popt.WgtDef)
                {
                    case Weight.Robust:
                        RobustEst(popt.RobustOpt,V,sigma, Qvv, P,ref PP, ref Wi);
                        break;
                    case Weight.RobustHelmert:
                        RobustHelmert(popt,V,sigma,Qvv,P,ref PP, ref Wi);
                        break;
                }
                //更新P1，P2
                P1 = PP.SubMatrix(0, nl, 0, nl);
                P2 = PP.SubMatrix(nl, ns, nl, ns);
                //重新计算
                N1 = A1.Transpose() * P1 * A1;
                N2 = A2.Transpose() * P2 * A2;
                W1 = A1.Transpose() * P1 * L1;
                W2 = A2.Transpose() * P2 * L2;
                N = N1 + N2;
                W = W1 + W2;
                X = N.Inverse() * W;
                V1 = A1 * X - L1;
                V2 = A2 * X - L2;
                V = V1.Stack(V2);
                A = A1.Stack(A2);
                Qvv = PP.Inverse() - A * N.Inverse() * A.Transpose();

                //坐标迭代
                foreach (KeyValuePair<string, int> dic in ukpIndex)
                {
                    Result[dic.Key].X += X[2 * dic.Value, 0] / 1000;
                    Result[dic.Key].Y += X[2 * dic.Value + 1, 0] / 1000;
                }
                var dx = X - Xpre;
                //double max = Max(ColArray( X.ToArray()))[0];
                double[] tmp = AdjCmn.Abs(ColArray(dx.ToArray()));
                double eps = Max(tmp)[0];
                if (eps < popt.Eps_X)
                {
                    VCE_t cov = new VCE_t
                    {
                        count = iter_VCE,
                        ql = qvl,
                        qs = qvs
                    };
                    cov_res.Add(cov);//存储方差分量估计的定权结果
                    break;
                }

                if (popt.WgtDef == Weight.DataSnooping) // 重新计算矩阵，重新组建
                {
                    mat Ac1 = Newmat(nl, t);mat Lc1 = Newmat(nl, 1);
                    mat Ac2 = Newmat(ns, t); mat Lc2 = Newmat(ns, 1);
                    Orientation_Angle(out DXJ);//计算定向角
                    GetFX1(DXJ, ref Ac1, ref Lc1);
                    GetJL1(ref Ac2, ref Lc2);
                    A1 = SplitMat2(Ac1, idxL.ToArray());
                    L1= SplitMat2(Lc1, idxL.ToArray());
                    A2 = SplitMat2(Ac2, idxS.ToArray());
                    L2 = SplitMat2(Lc2, idxS.ToArray());
                }
                else  // 抗差估计不需要重新组建矩阵
                {
                    //坐标迭代后，误差方程矩阵也跟着改变
                    Orientation_Angle(out DXJ);//计算定向角
                    GetFX1(DXJ, ref A1, ref L1);
                    GetJL1(ref A2, ref L2);
                }
                Xpre = X;
            }

            // 计算协因数和多余观测分量
            QX = (A.Transpose() * PP * A).Inverse();
            mat I;
            if (popt.WgtDef == Weight.DataSnooping) I = Eye(nn);
            else I = Eye(n);
            ROC = I - A * QX * A.Transpose() * PP;//多余观测分量Qvv*P
            //ROC = Qvv * PP;

            //  存储信息
            i = 0; Q1 = P1.Inverse();
            foreach (KeyValuePair<string, obs_dic> sta in obsL)
            {
                foreach (KeyValuePair<string, double> obs in sta.Value)
                {
                    int ids = FindStaName(sta.Key);
                    int ido = FindObsName(ids, obs.Key);
                    double v = (A1 * X)[i, 0] - L1[i, 0];
                    V1[i, 0] = v;
                    AllObs[ids].data[ido].vl = v;
                    AllObs[ids].data[ido].qL = Q1[i, i];
                    AllObs[ids].data[ido].rocL = ROC[i, i];
                    i++;
                }
            }
            i = 0; Q2 = P2.Inverse();
            foreach (KeyValuePair<string, obs_dic> sta in obsS)
            {
                foreach (KeyValuePair<string, double> obs in sta.Value)
                {
                    int ids = FindStaName(sta.Key);
                    int ido = FindObsName(ids, obs.Key);
                    double v = (A2 * X)[i, 0] - L2[i, 0];
                    V2[i, 0] = v;
                    AllObs[ids].data[ido].vs = v;
                    AllObs[ids].data[ido].qS = Q2[i, i];
                    AllObs[ids].data[ido].rocS = ROC[nl + i, nl + i];
                    i++;
                }
            }

            //计算验后单位权中误差
            if (popt.WgtDef>=Weight.Robust && popt.WgtDef !=Weight.DataSnooping)
            {
                nn = 0;n0 = 0;
                for (i = 0; i < n; i++)
                {
                    if (Wi[i, 0] == 1) n0++;    
                    if (Wi[i, 0] > Math.Pow(MIN,iter+1) )   // 每次都被判断为剔除,则剔除
                        nn++;
                }
                n1 = nn - n0; // n0:权不变，  n1:降权，   n2:剔除
                n2 = n - nn;
            }

            PVV1= (V1.Transpose() * P1 * V1)[0, 0];
            PVV2= (V2.Transpose() * P2 * V2)[0, 0];
            PVV = PVV1+PVV2;
            r = n - t;
            sigma = Math.Sqrt(PVV / r);
            fai = sigma; //验后单位权中误差

            Print("tr(ROC)=" + ROC.Trace());
            Print("iter counts : " + iter);
        }
    }
}
